#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/vector3.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <vector>
#include <cmath>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

class AvoidControl : public rclcpp::Node
{
public:
    AvoidControl()
    : Node("avoid")
    {
        initialize_parameters();
       
        this->pose.resize(this->vehicle_num);

        this->pose_sub.resize(this->vehicle_num);

        this->avoid_control_pub.resize(this->vehicle_num);

        this->vehicles_avoid_control.resize(this->vehicle_num, geometry_msgs::msg::Vector3());
        // this->avoid_radius =  5.0;
        this->alpha = 1.0;
        // Initialize subscribers and publishers
        for (int i = 0; i < vehicle_num; ++i)
        {
            std::string topic_name = vehicle_type + "_" + std::to_string(i) + "/vision_pose/pose";
            pose_sub[i] = this->create_subscription<geometry_msgs::msg::PoseStamped>(topic_name, rclcpp::QoS(10).best_effort(),
                [this, i](const geometry_msgs::msg::PoseStamped::SharedPtr msg) {
                    this->pose[i] = *msg;
                    // RCLCPP_INFO(this->get_logger(), "Received Pose: x=%f, y=%f", msg->pose.position.x, msg->pose.position.y);
                });

            
                std::string avoid_topic = namespaces + "/" + vehicle_type + "_" + std::to_string(i) + "/avoid_vel";
                avoid_control_pub[i] = this->create_publisher<geometry_msgs::msg::Vector3>(avoid_topic, 10);
           
        }

        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(50), std::bind(&AvoidControl::controlLoop, this));
    }
    void controlLoop();

    void initialize_parameters() {
        this->declare_parameter<std::string>("uav_namespace", "uav_swarms");
        this->declare_parameter<std::string>("vehicle_type", "drone");
        this->declare_parameter<int>("vehicle_num", 3);
        this->declare_parameter<double>("safe_distance", 6.0);

        vehicle_type = this->get_parameter("vehicle_type").as_string();
        namespaces = this->get_parameter("uav_namespace").as_string();
        vehicle_num = this->get_parameter("vehicle_num").as_int();  // 确保 vehicle_num 是整数
        avoid_radius = this->get_parameter("safe_distance").as_double();  // 确保 avoid_radius 是 double 类型

        RCLCPP_INFO(this->get_logger(), "Namespace: %s", namespaces.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle Type: %s", vehicle_type.c_str());
        RCLCPP_INFO(this->get_logger(), "Vehicle Num: %d", vehicle_num);
        RCLCPP_INFO(this->get_logger(), "Safe distance: %f", avoid_radius);

    }
    double calculateDistance(const Vector3d &pos1, const Vector3d &pos2) {
        return (pos1 - pos2).norm();
    }

    // 计算避障力（指数力模型）
    Vector3d calculateExponentialForce(const Vector3d &pos_i, const Vector3d &pos_j) {
        RCLCPP_INFO(this->get_logger(), "pos_i force: %f, %f, %f", pos_i.x(), pos_i.y(), pos_i.z());
        RCLCPP_INFO(this->get_logger(), "pos_j force: %f, %f, %f", pos_j.x(), pos_j.y(), pos_j.z());
        double distance = calculateDistance(pos_i, pos_j); // 两无人机的距离
        if (distance < avoid_radius) {
            // 计算力的方向：从 j 到 i
            Vector3d direction = pos_i - pos_j;
            // RCLCPP_INFO(this->get_logger(), "direction force: %f, %f, %f", direction.x(), direction.y(), direction.z());
            direction.normalize(); // 单位化方向向量

            // 计算避障力的大小
            double force_magnitude = exp(-alpha * (distance - avoid_radius));

            // 返回避障力
            return force_magnitude * direction;
        }
        return Vector3d(0.0,0.0, 0.0); // 如果超出安全距离，避障力为零
        
    }
private:
    // 无人机数量
    int vehicle_num;
    // 控制类型，vel或accel
    std::string control_type;
    std::string namespaces;
    std::string vehicle_type;
    // 避障半径
    double avoid_radius;
    std::vector<geometry_msgs::msg::PoseStamped> pose;
    // 无人机位置信息的订阅者
    std::vector<rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr> pose_sub;
    // 避障控制信息的发布者，用于发布控制速度的信息
    std::vector<rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr> avoid_control_pub;
    // 避障控制信息
    std::vector<geometry_msgs::msg::Vector3> vehicles_avoid_control;
    double alpha = 1.0;
    rclcpp::TimerBase::SharedPtr timer_;

    Vector3d aid_vec1 = {1, 0, 0};
    Vector3d aid_vec2 = {0, 1, 0};

    
};

